#include "motor.h"

void Motor_Init(void)
{
	/*    TsPu_Init     */
	SysPara.TsPU=11324; //q17
	
/*    Loop_Init     */
	Inside_Loop_Init();
	OutsideLoopInit();
/*   Timer_Init   */
	HAL_TIM_Base_Start(&htim1);
	HAL_TIM_Base_Start_IT(&htim6);	
  HAL_TIM_Encoder_Start(&htim4, TIM_CHANNEL_ALL);  	
/*   PWM_Init    */	
	Invter_Init();	
	Update_PWM();	
	PWMOutputs_DISABLE();
/*   ADC_Init    */	
		ADC_Init();
/*  Encoder_Init    */		
	Encoder_Control_Init();
/*  flux_Init    */	
	flux_pll_init();
/*  Wind_Init    */		
	Motor_ShortCircuit_Init();
/*  Key_Init    */			
SysPara.StateMachineFlag=Motor_StandBy;
/*  Ladrc_Init    */	
//	TD_Init(&ltd,0.001,100);
//	TD_Reset(&ltd);  
	Ladrc_setCtrlParm(&ladrc_vel,104 ,350, 512); //wc,w0,b0 
	Motor_state=IDLE;
/*  Speed_Init    */		
	Speed_Ramp_Init();
	
/*  Kalman_Init    */			

	KalmanInit(&kf,1,1,1000,0);

}

void FOC_ExecuteAndUpdatePWM(void)
{
			InsideLoopCalc();
				
			ipark1.Ds = IdLoop.Out;
			ipark1.Qs = IqLoop.Out;
			ipark1.Theta = SysPara.MotorAngle;

			IPARK_MACRO(&ipark1);
					
			PWM_Calc.ValfaPU_q14 = (ipark1.Alpha * PWM_Calc.PwmTBPRD)>>14;
			PWM_Calc.VbetaPU_q14 = (ipark1.Beta * PWM_Calc.PwmTBPRD)>>14;
					
			Invter_Calc();
					 
			PWM_Calc.UCMP = RangeLimit(0, PWM_Calc.UCMP, PWM_Calc.PwmTBPRD);
			PWM_Calc.VCMP = RangeLimit(0, PWM_Calc.VCMP, PWM_Calc.PwmTBPRD);
			PWM_Calc.WCMP = RangeLimit(0, PWM_Calc.WCMP, PWM_Calc.PwmTBPRD);	

			Update_PWM();	
	
}
